package us.oh.k12.wkw.robot.command;

import edu.wpi.first.wpilibj.DriverStation;
import edu.wpi.first.wpilibj.command.Command;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import us.oh.k12.wkw.robot.OperatorInterface;
import us.oh.k12.wkw.robot.subsystem.ClimbingSystem;
import us.oh.k12.wkw.robot.subsystem.DriveSystem;
import us.oh.k12.wkw.robot.subsystem.LaunchFrisbeeSystem;
import us.oh.k12.wkw.robot.util.WkwFrcLogger;

public abstract class CommandBase extends Command {

    // operator interface
    // NOTE: this is initialized in the init() method below.
    // and NOT here.
    private static OperatorInterface oi = null;
    // Create a single static instance of all of your subsystems
    // drive system
    // NOTE: this system must be initialized first because it checks all sensors
    // - and we need to do
    // this before the other systems allocate (lock) them.
    private static DriveSystem driveSystem = null;
    private static LaunchFrisbeeSystem launchSystem = null;
    private static ClimbingSystem climbSystem = null;
    private static double batteryVoltage = 0;

    //private static ClimbingSystem climbSystem = new ClimbingSystem();
    public static void init() {
        // This MUST be here. If the OI creates Commands (which it very likely
        // will), constructing it during the construction of CommandBase (from
        // which commands extend), subsystems are not guaranteed to be
        // yet. Thus, their requires() statements may grab null pointers. Bad
        // news. Don't move it.
        CommandBase.driveSystem = new DriveSystem();
        CommandBase.launchSystem = new LaunchFrisbeeSystem();
        CommandBase.climbSystem = new ClimbingSystem();

        CommandBase.oi = new OperatorInterface();

        // Show what command your subsystem is running on the SmartDashboard
        SmartDashboard.putData(CommandBase.driveSystem);
        SmartDashboard.putData(CommandBase.launchSystem);
        SmartDashboard.putData(CommandBase.climbSystem);
    }

    public static void flushStatus() {

        try {

            CommandBase.driveSystem.flushStatus();
            CommandBase.launchSystem.flushStatus();
            CommandBase.climbSystem.flushStatus();

        } catch (Exception anEx) {
            WkwFrcLogger.error(CommandBase.class.getName(), "flushStatus()", "Exception name="
                    + anEx.getClass().getName() + ", message=" + anEx.getMessage() + ".", anEx);
        }
    }

    public static void updateStatus() {

        try {

            if (Math.abs(CommandBase.batteryVoltage - DriverStation.getInstance().getBatteryVoltage()) > .01) {
                CommandBase.batteryVoltage = DriverStation.getInstance().getBatteryVoltage();
                SmartDashboard.putNumber("BatteryVoltage", DriverStation.getInstance().getBatteryVoltage());
            }

            CommandBase.driveSystem.updateStatus();
            CommandBase.launchSystem.updateStatus();
            CommandBase.climbSystem.updateStatus();

        } catch (Exception anEx) {
            WkwFrcLogger.error(CommandBase.class.getName(), "updateStatus()", "Exception name="
                    + anEx.getClass().getName() + ", message=" + anEx.getMessage() + ".", anEx);
        }
    }

    protected CommandBase() {
        super();
    }

    protected CommandBase(final String pName) {
        super(pName);
    }

    protected void initialize() {
        // no implementation
    }

    protected boolean isFinished() {
        // the default implementation if to end the command immediately.
        return true;
    }

    protected void end() {
        // no implementation
    }

    protected void interrupted() {
        // no implementation
    }

    protected OperatorInterface getOI() {
        return CommandBase.oi;
    }

    protected DriveSystem getDriveSystem() {
        return CommandBase.driveSystem;
    }

    protected LaunchFrisbeeSystem getLaunchFrisbeeSystem() {
        return CommandBase.launchSystem;
    }

    protected ClimbingSystem getClimbingSystem() {
        return CommandBase.climbSystem;
    }

    protected boolean isTimedOut() {
        boolean isTimedOut = super.isTimedOut();
        //if (isTimedOut) {
        //    this.reportCommandTimeout();
        //}
        return isTimedOut;
    }

    /*
     * 
     * support methods.
     */
    protected void debug(final String pMethod, final String pMessage) {
        //WkwFrcLogger.debug(this.getClassName(), pMethod, pMessage);
        System.out.println(pMessage);
    }

    protected void info(final String pMethod, final String pMessage) {
        //WkwFrcLogger.info(this.getClassName(), pMethod, pMessage);
        System.out.println(pMessage);
    }

    protected void reportCommandTimeout() {
        //WkwFrcLogger.info(this.getClassName(), "reportCommandTimeout()",
        //        " System:" + this.getName()
        //        + ", Sensor:N/A, State:N/A, Passed:false, Message:Command timed out.");
        System.out.print("System:" + this.getName()
                + ", Sensor:N/A, State:N/A, Passed:false, Message:Command timed out.");
    }

    private String formatException(final Exception pEx) {
        return (null == pEx ? "." : pEx.getClass().getName() + ", message=" + pEx.getMessage()
                + ".");
    }

    protected void error(final String pMethod, final Exception pEx) {
        this.error(pMethod, this.formatException(pEx), pEx);
    }

    protected void error(final String pMethod, final String pMessage, final Exception pEx) {
        //WkwFrcLogger.error(this.getClassName(), pMethod, pMessage, pEx);
        //if (null != pEx) {
        //    pEx.printStackTrace();
        //}
        System.out.println(pMessage);
    }

    protected String getClassName() {
        String aClassName = this.getClass().getName();
        return aClassName.substring(aClassName.lastIndexOf('.') + 1);
    }
}
